/*
 * @Author: greyqiu greyqiu@163.com
 * @Date: 2025-03-13 14:07:15
 * @LastEditors: greyqiu greyqiu@163.com
 * @LastEditTime: 2025-03-17 10:26:27
 * @Description:
 * 本模块代码改编自TI官方POWER_MEAS_SINE_ANALYZER;
 * 官方模块内部有除法运算，本模块将原有的功能拆分成采样和计算;
 * 其中采样运行在中断中，计算运行在main中。
 * Copyright (c) 2025 by ${greyqiu}, All Rights Reserved. 
 */
#include <stdint.h>
#ifndef __TMS320C28XX_CLA__
#include <math.h>
#else
#include <CLAmath.h> // 如果在CLA中运行，需要加入相关头文件CLAmath.h
#endif

#ifndef POWER_METER_H
#define POWER_METER_H

typedef struct {
    float v;           //!< Input: Voltage Sine Signal
    float i;           //!< Input Current Signal
    float sampleFreq;  //!< Input: Signal Sampling Freq
    float threshold;   //!< Input: Voltage level corresponding to zero i/p
    float vRms;        //!< Output: RMS Value
    float vAvg;        //!< Output: Average Value
    float vEma;        //!< Output: Exponential Moving Average Value
    float acFreq;      //!< Output: Signal Freq
    float acFreqAvg;   //!< Output: Signal Freq
    float iRms;        //!< Output: RMS Value of current
    float pRms;        //!< Output: RMS Value of input power
    float vaRms;       //!< Output: RMS VA
    float pRmsSlow;        //!< Output: RMS Value of input power
    float vaRmsSlow;       //!< Output: RMS VA
    float powerFactor; //!< Output: powerFactor
    int16_t  zcd1;          //!< Output: Zero Cross detected
    int16_t  zcd2;          //!< Output: Zero Cross detected
    
    float vSum;        //!< Internal : running sum for vac calculation over one sine cycles
    float vSumTmp;
    float vSqrSum;     //!< Internal : running sum for vacc square calculation over one sine cycle
    float vSqrSumTmp;
    float iSqrSum;     //!< Internal : running sum for Iacc_rms calculation over one sine cycle
    float iSqrSumTmp;
    float acFreqSum;   //!< Internal : running sum of acFreq
    float pSum;        //!< Internal : running sum for Pacc_rms calculation over one sine cycle
    float pSumTmp;
    float vaSumMul;    //!< Internal : running sum for Pacc_rms calculation over one sine cycle
    float vNorm;       //!< Internal: Normalized value of the input voltage
    float iNorm;       //!< Internal: Normalized value of the input current
    int16_t  prevSign;     //!< Internal: Flag to detect ZCD
    int16_t  currSign;     //!< Internal: Flag to detect ZCD
    int32_t  nSamples;     //!< Internal: No of samples in one cycle of the sine wave
    int32_t  nSamplesTmp;
    int32_t  nSamplesMin;  //!< Internal: Lowerbound for no of samples in one sine wave cycle
    int32_t  nSamplesMax;  //!< Internal: Upperbound for no of samples in one sine wave cycle
    float inverse_nSamples; //!< Internal: 1/( No of samples in one cycle of the sine wave)
    float sqrt_inverse_nSamples; //!< Internal: sqrt(1/( No of samples in one cycle of the sine wave))
    int16_t  slewPowerUpdate; //!< Internal: used to slew update of the power value
    float pRmsSumMul; //!< Internal: used to sum Pac value over multiple sine cycles (100)
    int16_t jitterCount; //!< Internal: used to store jitter information due to noise on input
    float emaFilterMultiplier;  //!< Internal: multiplier value used for the exponential moving average filter
} PowerMeter;

//! \brief Resets internal data to zero
//! \param *v The POWER_MEAS structure pointer
//! \return None
//!
static inline void powerMeterReset(PowerMeter *v)
{
    v->vRms=0;
    v->vAvg=0;
    v->vEma=0;
    v->acFreq=0;
    v->iRms=0;
    v->pRmsSlow=0;
    v->vaRmsSlow=0;
    v->powerFactor=0;
    v->zcd1=0;
    v->zcd2=0;
    v->vSum=0;
    v->vSqrSum=0;
    v->iSqrSum=0;
    v->pSum=0;
    v->vaSumMul=0;
    v->vNorm=0;
    v->iNorm=0;
    v->prevSign=0;
    v->currSign=0;
    v->nSamples=0;
    v->nSamplesMin = 0;
    v->nSamplesMax = 0;
    v->inverse_nSamples=0;
    v->sqrt_inverse_nSamples=0;
    v->pRmsSumMul=0;
    v->acFreqSum=0;
    v->acFreqAvg=0;
    v->jitterCount=0;
    v->emaFilterMultiplier=0;
}

//! \brief Configures the power measurment module
//! \param *v The POWER_MEAS structure pointer
//! \param isrFrequency Frequency at which SPLL module is run
//! \param threshold Threshold value to avoid zero crossing issues
//! \param gridMaxFreq Max grid frequency
//! \param gridMinFreq Min grid frequency
//! \return None
//!
static inline void powerMeterConfig(PowerMeter *v,
                                    float isrFrequency,
                                    float threshold,
                                    float gridMaxFreq,
                                    float gridMinFreq)
{
    v->sampleFreq = (float)(isrFrequency);
    v->threshold = (float)(threshold);
    v->nSamplesMax=isrFrequency/gridMinFreq;
    v->nSamplesMin=isrFrequency/gridMaxFreq;
    v->emaFilterMultiplier=2.0f/isrFrequency;
}

//! \brief Perform calculations using the POWER_MEAS_SINE_ANALYZER module
//! \param *v The POWER_MEAS structure pointer
//! \return None
//!
static inline void powerMeterSample(PowerMeter *v)
{
    v->vNorm = fabsf(v->v);
    v->iNorm = fabsf(v->i);
    v->currSign = ( v->v > v->threshold) ? 1 : 0;
    v->nSamplesTmp++;
    v->vSumTmp = v->vSumTmp + v->vNorm;
    v->vSqrSumTmp = v->vSqrSumTmp + (v->vNorm * v->vNorm);
    v->iSqrSumTmp = v->iSqrSumTmp + (v->iNorm * v->iNorm);
//    v->vEma = v->vEma+(v->emaFilterMultiplier*(v->vNorm - v->vEma));
    v->pSumTmp = v->pSumTmp + (v->i*v->v);
    // v->zcd=0;

    if((v->prevSign != v->currSign) && (v->currSign == 1))
    {
        //
        // check if the nSamples are in the ball park of a real frequency
        // that can be on the grid, this is done by comparing the nSamples
        // with the max value and min value it can be for the 
        // AC Grid Connection these Max and Min are initialized by the 
        // user in the code
        //
        if(v->nSamplesMin < v->nSamplesTmp )
        {
            v->zcd1=1; // 过零标志职位，powerMeterCaculate检测到此标志后会执行有效值等计算
            v->zcd2=1;
            v->nSamples = v->nSamplesTmp;
            v->vSum = v->vSumTmp;
            v->vSqrSum = v->vSqrSumTmp;
            v->iSqrSum = v->iSqrSumTmp;
            v->pSum = v->pSumTmp;

            v->jitterCount=0;

            v->nSamplesTmp=0;
            v->vSumTmp=0;
            v->vSqrSumTmp=0;
            v->iSqrSumTmp=0;
            v->pSumTmp =0;
        }
        else
        {
            //
            // otherwise it may be jitter ignore this reading
            // but count the number of jitters you are getting
            // but do not count to infinity as then when the grid comes back
            // it will take too much time to wind down the jitter count
            //
            if(v->jitterCount<30)
            {
                v->jitterCount++;
            }
            v->nSamplesTmp=0;
        }
    }

    if(v->nSamplesTmp > v->nSamplesMax || v->jitterCount > 20)
    {
        if (v->zcd1==0) {
            //
            // most certainly the AC voltage is not present
            //
            v->vRms = 0;
            v->vAvg = 0;
            v->vEma = 0;
            v->acFreq=0;
            v->iRms = 0;
            v->pRmsSlow = 0;
            v->vaRmsSlow =0;
            v->powerFactor=0;

            v->zcd1=0;
            v->zcd2=0;
            v->vSum=0;
            v->vSqrSum=0;
            v->iSqrSum=0;
            v->pSum=0;
            v->vaSumMul=0;
            v->pRmsSumMul = 0;
            v->acFreqAvg = 0;
            v->acFreqSum =0 ;
            v->nSamples=0;
            v->jitterCount=0;

            v->nSamplesTmp=0;
            v->vSumTmp=0;
            v->vSqrSumTmp=0;
            v->iSqrSumTmp=0;
            v->pSumTmp =0;
        }
    }

    v->prevSign = v->currSign;
}


static inline void powerMeterCaculate(PowerMeter *v)
{
    if (v->zcd1 == 1) { // 改为外部自行判断
        v->inverse_nSamples = (1.0f)/(v->nSamples);
        v->sqrt_inverse_nSamples = sqrtf(v->inverse_nSamples);
        v->vAvg = (v->vSum*v->inverse_nSamples);
        v->vRms = sqrtf(v->vSqrSum)*v->sqrt_inverse_nSamples;
        v->iRms = sqrtf(v->iSqrSum)*v->sqrt_inverse_nSamples;
        v->pRms = (v->pSum*v->inverse_nSamples);
        v->pRmsSumMul = v->pRmsSumMul + v->pRms;
        v->vaRms = v->vRms*v->iRms;
        v->vaSumMul = v->vaSumMul + v->vaRms;
        v->acFreq = (v->sampleFreq*v->inverse_nSamples);
        v->acFreqSum = v->acFreqSum + v->acFreq;

        v->slewPowerUpdate++;

        if(v->slewPowerUpdate >= 100)
        {
            v->slewPowerUpdate=0;
            v->pRmsSlow = (v->pRmsSumMul*(0.01f));
            v->pRmsSumMul = 0;
            v->vaRmsSlow = v->vaSumMul * (0.01f);
            v->vaSumMul = 0;
            v->powerFactor=v->pRmsSlow/v->vaRmsSlow;
            v->acFreqAvg=v->acFreqSum*0.01f;
            v->acFreqSum=0;
        }
        v->zcd1 = 0;
    }
}

#endif // POWER_METER_H
